#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#ifdef __cplusplus
extern "C" {
#endif
#include "cvapi_ambacv_flexidag.h"
#include "cvapi_svccvalgo_memio_interface.h"
#include "schdr_api.h"
#ifdef __cplusplus
}
#endif

#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>
#include <boost/property_tree/ini_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <iostream>
#include <unordered_map>

#include "DataManager.h"
#include "bev_fusion.h"
#include "bev_net.h"
#include "common/file_helper.h"
#include "common/infer_common.h"
#include "common/io_helper.h"
#include "grid_sample.h"
#include "image_encoder.h"
#include "postprocess/post_processor.h"
#include "utils.h"

static void Do_Prepare_Config(AMBA_CV_STANDALONE_SCHDR_CFG_s* cfg) {
  cfg->flexidag_slot_num = 8;
  cfg->cavalry_slot_num = 0;
  cfg->cpu_map = 0xD;
  cfg->log_level = LVL_DEBUG;
  cfg->boot_cfg = 1 + FLEXIDAG_ENABLE_BOOT_CFG;
}

static uint32_t Do_SchdrStart(void) {
  const char orcvp_path[] = "/lib/firmware/";
  uint32_t schdr_state;
  uint32_t Rval = 0U;

  Rval = AmbaCV_SchdrState(&schdr_state);
  if (Rval == 0U) {
    if (schdr_state == FLEXIDAG_SCHDR_OFF) {
      AMBA_CV_STANDALONE_SCHDR_CFG_s cfg = {0};

      Rval = AmbaCV_SchdrLoad(orcvp_path);
      if (Rval == 0U) {
        Do_Prepare_Config(&cfg);
        Rval = AmbaCV_StandAloneSchdrStart(&cfg);
      }
    } else {
      AMBA_CV_FLEXIDAG_SCHDR_CFG_s cfg;

      cfg.cpu_map = 0xD;
      cfg.log_level = LVL_DEBUG;
      Rval = AmbaCV_FlexidagSchdrStart(&cfg);
    }
  }

  if (Rval != 0U) {
    printf("Do_SchdrStart fail (0x%x)\n", Rval);
  }

  return Rval;
}

std::pair<std::uint16_t, std::uint16_t> get_dims(std::string const& dims) {
  auto i = dims.find(",");
  std::uint16_t first = atoi(dims.substr(0, i).c_str());
  std::uint16_t second = atoi(dims.substr(i + 1, dims.size() - i).c_str());

  return {first, second};
}

bool get_out_location(boost::property_tree::ptree& root_node,
                      BevFusion::Config& config) {
  auto out_data = root_node.get_child("OUT");
  config.type_size = out_data.get<int>("type_size");
  int count = out_data.get<int>("count");

  auto box_size = root_node.get_child("box_size");
  auto box_dims = get_dims(box_size.get<std::string>("box_dims"));
  auto box_rot_dims = get_dims(box_size.get<std::string>("box_rot_dims"));
  auto box_vel_dims = get_dims(box_size.get<std::string>("box_vel_dims"));
  auto box_cls_dims = get_dims(box_size.get<std::string>("box_cls_dims"));
  auto box_reg_dims = get_dims(box_size.get<std::string>("box_reg_dims"));
  auto box_height_dims = get_dims(box_size.get<std::string>("box_height_dims"));

  auto map_size = root_node.get_child("map_size");
  auto map_reg_dims = get_dims(map_size.get<std::string>("map_reg_dims"));
  auto map_cls_dims = get_dims(map_size.get<std::string>("map_cls_dims"));

  std::unordered_map<std::string, std::int8_t> feature_ids{
      {"reg", 0}, {"cls", 1}, {"dim", 2}, {"rot", 3}, {"vel", 4}, {"hei", 5}};

  for (int i = 0; i < count; ++i) {
    std::uint32_t start = 0;
    std::string out_name = "out" + std::to_string(i);
    auto const& info = root_node.get_child(out_name);
    for (auto const& val : info) {
      int array_id = (val.first.at(4) - '0');
      std::string feature_name = val.first.substr(6, 3);
      if (val.first.substr(0, 3) == "box") {
        std::pair<uint16_t, uint16_t>* dims = nullptr;
        if (feature_name == "reg") {
          dims = &box_reg_dims;
        }
        if (feature_name == "cls") {
          dims = &box_cls_dims;
        }
        if (feature_name == "dim") {
          dims = &box_dims;
        }
        if (feature_name == "rot") {
          dims = &box_rot_dims;
        }
        if (feature_name == "vel") {
          dims = &box_vel_dims;
        }
        if (feature_name == "hei") {
          dims = &box_height_dims;
        } else {
          ;
        }

        int fea_id = feature_ids[feature_name];
        auto& box = config.out_location.box_loc[array_id];
        auto& feature = box[fea_id];
        feature.out_id = i;
        feature.start = start;
        feature.prev_dim = dims->first;
        feature.last_dim = dims->second;
        feature.size = dims->first * dims->second * config.type_size;
        start += feature.size;
      } else if (val.first.substr(0, 3) == "map") {
        std::pair<uint16_t, uint16_t>* dims = nullptr;
        if (feature_name == "reg") {
          dims = &map_reg_dims;
        }
        if (feature_name == "cls") {
          dims = &map_cls_dims;
        }

        int fea_id = feature_ids[feature_name];
        auto& map = config.out_location.map_loc[array_id];
        auto& feature = map[fea_id];
        feature.out_id = i;
        feature.start = start;
        feature.prev_dim = dims->first;
        feature.last_dim = dims->second;
        feature.size = dims->first * dims->second * config.type_size;
        start += feature.size;
      }
    }
  }
}

BevFusion::Config read_config(std::string& config_path) {
  if (!boost::filesystem::exists(config_path)) {
    std::cerr << "config.ini not exists." << std::endl;
    return {};
  }
  boost::property_tree::ptree root_node, tag_system;
  boost::property_tree::ini_parser::read_ini("config.ini", root_node);
  tag_system = root_node.get_child("ModelPath");
  if (tag_system.count("image_encoder_path") != 1) {
    std::cerr << "image_encoder_path node not exists." << std::endl;
    return {};
  }
  BevFusion::Config config;
  config.bev_net_path = tag_system.get<std::string>("bev_net_path");
  config.image_encoder_path = tag_system.get<std::string>("image_encoder_path");
  config.grid_sample_path = tag_system.get<std::string>("grid_sample_path");
  config.input_dir = tag_system.get<std::string>("input_dir");

  std::cout << "bev_net_path: " << config.bev_net_path << std::endl;
  int image_size = tag_system.get<int>("image_size");
  std::cout << "image_size = " << image_size << std::endl;

  tag_system = root_node.get_child("Data");
  config.input_image_path = tag_system.get<std::string>("input_image_path");
  config.pre_input_path = tag_system.get<std::string>("pre_input_path");
  config.align_matrix_path = tag_system.get<std::string>("align_matrix_path");
  config.ring_matrix_path = tag_system.get<std::string>("ring_matrix_path");
  config.ray_matrix_path = tag_system.get<std::string>("ray_matrix_path");
  config.lidar_input_path = tag_system.get<std::string>("lidar_input_path");
  if (tag_system.count("kCat2491_path") > 0) {
    config.kCat2491_path = tag_system.get<std::string>("kCat2491_path");
  }

  if (tag_system.count("kCat2492_path") > 0) {
    config.kCat2492_path = tag_system.get<std::string>("kCat2492_path");
  }

  if (tag_system.count("k2647_path") > 0) {
    config.k2647_path = tag_system.get<std::string>("k2647_path");
  }

  // for (auto const& val : out_data) {
  //   config.out_index.emplace(val.first, std::stoi(val.second.data()));
  //   std::cout << val.first << " = [" << val.second.data() << "]" <<
  //   std::endl;
  // }
  return config;
}

#if 0
int test2(void) {
  DataManagerSettings dataManagerSettings{
      .data_scene_paths = {"/home/datasets/nuscenes_formated/scene-0017",
                           "/home/datasets/nuscenes_formated/scene-0092",
                           "/home/datasets/nuscenes_formated/scene-0097",
                           "/home/datasets/nuscenes_formated/scene-0101",
                           "/home/datasets/nuscenes_formated/scene-0110"}};

  auto& dataManager{DataManager::GetInstance()};
  dataManager.Init(dataManagerSettings);

  PostProcessorSettings postProcessorSettings;
  auto& postProcessor{PostProcessor::GetInstance()};
  std::string error;
  bool bRet = postProcessor.Init(postProcessorSettings, error);
  if (bRet != true) {
    std::cout << "[" << to_red("ERROR") << "] "
              << "PostProcessor init fail, " + error << std::endl;
    return false;
  }

  auto const& sceneNumber{dataManager.GetDataSceneCount()};
  uint16_t currentSceneIndex = 0;
  DataScene* currenDataScenePtr = nullptr;

  while (true) {
    if (currenDataScenePtr == nullptr) {
      auto& scene{dataManager.GetDataScene(currentSceneIndex)};
      currenDataScenePtr = &scene;
      currenDataScenePtr->Play();
    } else {
      if (currenDataScenePtr->IsFrameRunOut()) {
        currentSceneIndex++;
        if (currentSceneIndex >= sceneNumber) {
          currentSceneIndex = 0;
        }
        auto& scene{dataManager.GetDataScene(currentSceneIndex)};
        currenDataScenePtr = &scene;
        currenDataScenePtr->Play();
      }
    }

    if (currenDataScenePtr == nullptr) {
      std::cout << "[" << to_red("ERROR") << "] "
                << "currenDataScenePtr is nullptr, " << std::endl;
      return false;
    }

    int32_t process_count = 0;
    std::shared_ptr<CircularBuffer<PerceptionResult, 100>>
        fusionPerceptionResultBufferPtr =
            std::make_shared<CircularBuffer<PerceptionResult, 100>>();

    fusionPerceptionResultBufferPtr->Clear();

    while (true) {
      if (currenDataScenePtr->IsFrameRunOut()) {
        std::cout << to_yellow("[INFO] ") << "switch scene ......" << std::endl;
        while (true) {
          std::chrono::milliseconds dura(1000);
          std::this_thread::sleep_for(dura);
        }
        break;
      }

      if (currenDataScenePtr->IsEmpty()) {
        std::cout << "waiting data!!!!!!!!!!!!" << std::endl;
        std::chrono::milliseconds dura(1000);
        std::this_thread::sleep_for(dura);
        continue;
      }

      auto& fusionPerceptionResult{fusionPerceptionResultBufferPtr->GetNext()};
      DataFrame const& dataFrame{currenDataScenePtr->RetrieveDataFrame()};
    }
  }

  return 0;
}

#endif

std::int32_t GetFileNames(const std::string& dir,
                          std::vector<std::string>& filenames) {
  boost::filesystem::path path(dir);
  if (!boost::filesystem::exists(path)) {
    return -1;
  }

  if (!boost::filesystem::is_directory(path)) {
    return -1;
  }

  boost::filesystem::directory_iterator dataIter(path);
  for (auto const& data : dataIter) {
    if (boost::filesystem::is_regular_file(data.status())) {
      filenames.emplace_back(data.path().filename().string());
    }
  }
  std::sort(filenames.begin(), filenames.end());
  return filenames.size();
}

// image [1,3,256,704], fp16  704 * 2 / 128 = 11 ok
// pre_input, 1,256,128,128 ok

// align_matrix ,128,128,2 not ok
// constexpr std::uint32_t align_matrix_size = 128 * 128 * 128;
std::pair<char*, std::uint64_t> GetDataPad32(
    char* data, std::pair<std::uint32_t, std::uint32_t> dims,
    std::uint8_t type_size) {
  std::uint32_t old_row_size = dims.second * type_size;
  if (old_row_size % 128 == 0) {
    return {data, old_row_size * dims.first};
  }

  std::uint32_t new_row_size = (old_row_size / 128 + 1) * 128;
  std::uint64_t new_size = new_row_size * dims.first;

  char* data_pad = (char*)malloc(new_size);
  memset(data_pad, 0, new_size);

  int new_index = 0;
  int num = 0;
  while (new_index < new_size) {
    memcpy(data_pad + new_index, data, old_row_size);
    new_index += new_row_size;
    data += old_row_size;
  }
  return {data_pad, new_size};
}

DataBuf GetDataBuf(BevFusion::Config const& config) {
  DataBuf data_frame;
  std::uint8_t size_type = 2;
  // lidar_input_path 4* 2048,2048
  auto binSize{get_binfile_size(config.lidar_input_path)};
  assert(binSize == 4 * 2048 * 2048 * 2);
  char* buf{new char[binSize]};
  read_bin_file(config.lidar_input_path, buf, binSize);
  data_frame.lidar_data = GetDataPad32(buf, {2048, 2048}, 2);
  if (buf != data_frame.lidar_data.first) {
    delete buf;
  }

  // pre_input type: [1,256,128,128]
  binSize = get_binfile_size(config.pre_input_path);
  assert(binSize == 256 * 128 * 128 * size_type);
  buf = new char[binSize];
  read_bin_file(config.pre_input_path, buf, binSize);
  data_frame.pre_input = GetDataPad32(buf, {256 * 128U, 128U}, size_type);
  if (buf != data_frame.pre_input.first) {
    delete buf;
  }

  // 128, 128, 2
  binSize = get_binfile_size(config.align_matrix_path);
  assert(binSize == 128 * 128 * 2 * size_type);
  buf = new char[binSize];
  read_bin_file(config.align_matrix_path, buf, binSize);
  data_frame.align_matrix = GetDataPad32(buf, {128 * 128U, 2U}, size_type);
  if (buf != data_frame.align_matrix.first) {
    delete buf;
  }

  // ring_matrix [1,59,16384]
  binSize = get_binfile_size(config.ring_matrix_path);
  assert(binSize == 59 * 16384 * size_type);
  buf = new char[binSize];
  read_bin_file(config.ring_matrix_path, buf, binSize);
  data_frame.ring_matrix = GetDataPad32(buf, {59U, 16384U}, size_type);
  if (buf != data_frame.ring_matrix.first) {
    delete buf;
  }

  // ray_matrix 264,16384
  binSize = get_binfile_size(config.ray_matrix_path);
  assert(binSize == 264 * 16384 * size_type);
  buf = new char[binSize];
  read_bin_file(config.ray_matrix_path, buf, binSize);
  data_frame.ray_matrix = GetDataPad32(buf, {264U, 16384U}, size_type);
  if (buf != data_frame.ray_matrix.first) {
    delete buf;
  }

  // img_input0 3,256,704
  binSize = get_binfile_size(config.input_image_path);
  assert(binSize == 3 * 256 * 704 * size_type);
  buf = new char[binSize];
  read_bin_file(config.input_image_path, buf, binSize);
  data_frame.camera_data[0] = GetDataPad32(buf, {3 * 256U, 704U}, size_type);
  if (buf != data_frame.ray_matrix.first) {
    delete buf;
  }

  if (config.kCat2491_path.length() > 1) {
    binSize = get_binfile_size(config.kCat2491_path);
    assert(binSize == 264 * 59 * size_type);
    buf = new char[binSize];
    read_bin_file(config.input_image_path, buf, binSize);
    data_frame.kCat2491 = GetDataPad32(buf, {264U, 59U}, size_type);
    if (buf != data_frame.kCat2491.first) {
      delete buf;
    }
  }

  if (config.kCat2492_path.length() > 1) {
    binSize = get_binfile_size(config.kCat2492_path);
    assert(binSize == 264 * 80 * size_type);
    buf = new char[binSize];
    read_bin_file(config.input_image_path, buf, binSize);
    data_frame.kCat2492 = GetDataPad32(buf, {264U, 80U}, size_type);
    if (buf != data_frame.kCat2492.first) {
      delete buf;
    }
  }

  // 256,128,128
  if (config.k2647_path.length() > 1) {
    binSize = get_binfile_size(config.k2647_path);
    assert(binSize == 256 * 128 * 128 * size_type);
    buf = new char[binSize];
    read_bin_file(config.input_image_path, buf, binSize);
    data_frame.k2647 = GetDataPad32(buf, {256 * 128U, 128U}, size_type);
    if (buf != data_frame.k2647.first) {
      delete buf;
    }
  }

  return data_frame;
}

int main() {
  // 开启 scheduler
  std::uint32_t Rval = Do_SchdrStart();
  if (Rval != 0U) {
    printf("Do_SchdrStart fail (0x%x)\n", Rval);
    return 1;
  }

  std::string config_path = "./config.ini";
  BevFusion::Config config = read_config(config_path);

  DataBuf data_buf = GetDataBuf(config);
  printf("load data finish .....\n");

#if 0
  GridSample grid_sample;
  Rval = grid_sample.Init(config.grid_sample_path);
  if (Rval != 0U) {   
    return 1;
  }
  Rval = grid_sample.Run(data_buf);
  if (Rval != 0U) {
    printf("grid_sample run error .....\n");   
    return 1;
  }
  printf("grid_sample end .....\n");

  BevNet bev_net;
  Rval = bev_net.Init(config.bev_net_path);
  if (Rval != 0U) {   
    return 1;
  }
  bev_net.Run(data_buf);
  printf("bev_net end .....\n");
#endif

  // DataScene data_scene;
  // data_scene.Init();

  // std::vector<std::string> filenames;
  // std::uint32_t frame_num = GetFileNames(config.lidar_input_path, filenames);
  // while (true) {
  // }

#if 1
  BevFusion bev_fusion;
  Rval = bev_fusion.Init(config);
  if (Rval != 0U) {
    printf("bev_fusion init fail (0x%x)\n", Rval);
    return 1;
  }
  bev_fusion.Run(data_buf);

  // bev_fusion.Test(config);
  // DataFrame data_frame;
  // bev_fusion.Run(data_frame);
#endif
}
